/*******************************************************************************
* This file provides the API for the following sensors:
* ~ ADXL345
* ~ ITG-3200
* ~ HMC5843
*
* Author: Kong Wai Weng
* Email:  waiweng83@gmail.com
*******************************************************************************/



#include "hal_sensors.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/

// I2C slave address.
#define I2C_ADD_ACC		0x53	// ADXL345.
#define I2C_ADD_GYRO	0x68	// ITG-3200.
#define I2C_ADD_MAG		0x1e	// HMC5843.

// ADXL345 register address.
#define ACC_BW_RATE		0x2c
#define ACC_POWER_CTL	0x2d
#define ACC_X			0x32	// Little Endian.
#define ACC_Y			0x34	// Little Endian.
#define ACC_Z			0x36	// Little Endian.

// ITG-3200 register address.
#define GYRO_DLPF_FS	0x16
#define GYRO_TEMP		0x1b	// Big Endian.
#define GYRO_X			0x1d	// Big Endian.
#define GYRO_Y			0x1f	// Big Endian.
#define GYRO_Z			0x21	// Big Endian.

// HMC5843 register address.
#define MAG_CONFIG_A	0x00
#define MAG_MODE		0x02
#define MAG_X			0x03	// Big Endian.
#define MAG_Y			0x05	// Big Endian.
#define MAG_Z			0x07	// Big Endian.



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/



/*******************************************************************************
* PUBLIC FUNCTION: vInitSensors
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Initialize the sensors.
* This function must be called after the I2C has been initialized.
*
*******************************************************************************/
void vInitSensors(void)
{
	// #####################################################################
	// Initialize the ADXL345 Accelerometer.
	// #####################################################################
	
	// Output Data Rate = 800Hz.
	unsigned char ucData;
	ucData = 0x0d;
	if (uiWriteI2c1(I2C_ADD_ACC, ACC_BW_RATE, 1, &ucData) != 0) {
		xSystemError.bAccError = 1;
	}	
	
	// Start measuring.
	ucData = 0x08;
	if (uiWriteI2c1(I2C_ADD_ACC, ACC_POWER_CTL, 1, &ucData) != 0) {
		xSystemError.bAccError = 1;
	}	
	
	
	
	// #####################################################################
	// Initialize the ITG-3200 Gyro.
	// #####################################################################
	
	// Full Scale = 2000deg/sec.
	// Digital Low Pass Filter = 10Hz.
	// Internal Sampling Rate = 1kHz.
	ucData = 0b00011101;
	if (uiWriteI2c1(I2C_ADD_GYRO, GYRO_DLPF_FS, 1, &ucData) != 0) {
		xSystemError.bGyroError = 1;
	}	
	
	
	
	// #####################################################################
	// Initialize the HMC5843 Magnetometer.
	// #####################################################################
	
	// Data rate = 50Hz.
	ucData = 0b00011000;
	if (uiWriteI2c1(I2C_ADD_MAG, MAG_CONFIG_A, 1, &ucData) != 0) {
		xSystemError.bMagError = 1;
	}	
	
	// Change to Continuous-Conversion Mode.
	ucData = 0b00000000;
	if (uiWriteI2c1(I2C_ADD_MAG, MAG_MODE, 1, &ucData) != 0) {
		xSystemError.bMagError = 1;
	}	
	
	
	
	// #####################################################################
	// #####################################################################
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadAccRaw
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The raw data from the accelerometer.
*
* DESCRIPTIONS:
* Read the raw data from the accelerometer.
*
*******************************************************************************/
struct ACC_DATA_RAW xReadAccRaw(void)
{
	struct ACC_DATA_RAW xAccDataRaw;
	
	// Read the accelerometer data from I2C 1.
	if (uiReadI2c1(I2C_ADD_ACC, ACC_X, 6, (unsigned char*)&xAccDataRaw) != 0) {
		xSystemError.bAccError = 1;
	}	
	
	// Align the axis.
	xAccDataRaw.siAccX = -xAccDataRaw.siAccX;
	xAccDataRaw.siAccZ = -xAccDataRaw.siAccZ;
	
	// Return the data.
	return xAccDataRaw;
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadAcc
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The scaled and calibrated accelerometer data.
* ~ Unit: G (9.8m/s).
*
* DESCRIPTIONS:
* Read the scaled and calibrated accelerometer data.
* System config must be loaded into RAM before calling this function.
*
*******************************************************************************/
struct ACC_DATA xReadAcc(void)
{
	struct ACC_DATA_RAW xAccDataRaw;
	struct ACC_DATA xAccData;
	
	// Read the raw data.
	xAccDataRaw = xReadAccRaw();
	
	// Scale and calibrate the data.
	xAccData.fAccX = (float)(xAccDataRaw.siAccX - xSystemConfig.siAccXMid) * xSystemConfig.fAccXScale;
	xAccData.fAccY = (float)(xAccDataRaw.siAccY - xSystemConfig.siAccYMid) * xSystemConfig.fAccYScale;
	xAccData.fAccZ = (float)(xAccDataRaw.siAccZ - xSystemConfig.siAccZMid) * xSystemConfig.fAccZScale;
	
	return xAccData;
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadGyroRaw
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The raw data from the gyro.
*
* DESCRIPTIONS:
* Read the raw data from the gyro.
*
*******************************************************************************/
struct GYRO_DATA_RAW xReadGyroRaw(void)
{
	struct GYRO_DATA_RAW xGyroDataRaw;
	unsigned char aucBuffer[6];
	
	// Read the gyro data from I2C 1.
	if (uiReadI2c1(I2C_ADD_GYRO, GYRO_X, 6, aucBuffer) != 0) {
		xSystemError.bGyroError = 1;
	}	
	
	// Convert the data into signed int.
	// Align the axis too.
	xGyroDataRaw.siGyroX = 0 - (signed int)(((unsigned int)aucBuffer[0] << 8) + (unsigned int)aucBuffer[1]);
	xGyroDataRaw.siGyroY = (signed int)(((unsigned int)aucBuffer[2] << 8) + (unsigned int)aucBuffer[3]);
	xGyroDataRaw.siGyroZ = 0 - (signed int)(((unsigned int)aucBuffer[4] << 8) + (unsigned int)aucBuffer[5]);
	
	// Return the data.
	return xGyroDataRaw;
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadGyro
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The scaled and calibrated gyro data.
* ~ Unit: rad/s.
*
* DESCRIPTIONS:
* Read the scaled and calibrated gyro data.
* System config must be loaded into RAM before calling this function.
*
*******************************************************************************/
struct GYRO_DATA xReadGyro(void)
{
	struct GYRO_DATA_RAW xGyroDataRaw;
	struct GYRO_DATA xGyroData;
	
	// Read the raw data.
	xGyroDataRaw = xReadGyroRaw();
	
	// Scale and calibrate the data.
	xGyroData.fGyroX = (float)(xGyroDataRaw.siGyroX - xSystemConfig.siGyroXMid) * xSystemConfig.fGyroXScale;
	xGyroData.fGyroY = (float)(xGyroDataRaw.siGyroY - xSystemConfig.siGyroYMid) * xSystemConfig.fGyroYScale;
	xGyroData.fGyroZ = (float)(xGyroDataRaw.siGyroZ - xSystemConfig.siGyroZMid) * xSystemConfig.fGyroZScale;
	
	return xGyroData;
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadMagRaw
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The raw data from the magnetometer.
*
* DESCRIPTIONS:
* Read the raw data from the magnetometer.
*
*******************************************************************************/
struct MAG_DATA_RAW xReadMagRaw(void)
{
	struct MAG_DATA_RAW xMagDataRaw;
	unsigned char aucBuffer[6];
	
	// Read the magnetometer data from I2C 1.
	if (uiReadI2c1(I2C_ADD_MAG, MAG_X, 6, aucBuffer) != 0) {
		xSystemError.bMagError = 1;
	}	
	
	// Convert the data into signed int.
	xMagDataRaw.siMagY = 0 - (signed int)(((unsigned int)aucBuffer[0] << 8) + (unsigned int)aucBuffer[1]);
	xMagDataRaw.siMagX = 0 - (signed int)(((unsigned int)aucBuffer[2] << 8) + (unsigned int)aucBuffer[3]);
	xMagDataRaw.siMagZ = 0 - (signed int)(((unsigned int)aucBuffer[4] << 8) + (unsigned int)aucBuffer[5]);
	
	// Return the data.
	return xMagDataRaw;
}	



/*******************************************************************************
* PUBLIC FUNCTION: xReadMag
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ The scaled and calibrated magnetometer data.
* ~ Value is scaled to within -1 to +1.
*
* DESCRIPTIONS:
* Read the scaled and calibrated magnetometer data.
* System config must be loaded into RAM before calling this function.
*
*******************************************************************************/
struct MAG_DATA xReadMag(void)
{
	struct MAG_DATA_RAW xMagDataRaw;
	struct MAG_DATA xMagData;
	
	// Read the raw data.
	xMagDataRaw = xReadMagRaw();
	
	// Scale and calibrate the data.
	xMagData.fMagX = (float)(xMagDataRaw.siMagX - xSystemConfig.siMagXMid) * xSystemConfig.fMagXScale;
	xMagData.fMagY = (float)(xMagDataRaw.siMagY - xSystemConfig.siMagYMid) * xSystemConfig.fMagYScale;
	xMagData.fMagZ = (float)(xMagDataRaw.siMagZ - xSystemConfig.siMagZMid) * xSystemConfig.fMagZScale;
	
	return xMagData;
}	



/*******************************************************************************
* PUBLIC FUNCTION: vCalibrateMag
*
* PARAMETERS:
* ~ void
*
* RETURN:
* ~ void
*
* DESCRIPTIONS:
* Calibrate the magnetometer.
*
*******************************************************************************/
void vCalibrateMag(void)
{
	signed int siMaxX = 0;
	signed int siMaxY = 0;
	signed int siMaxZ = 0;
	
	signed int siMinX = 0;
	signed int siMinY = 0;
	signed int siMinZ = 0;
	
	
	
	// Push and release the switch to begin.
	while (PUSH_BUTTON == 1) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}
	}	
	while (PUSH_BUTTON == 0) {
		// If we are calling from task, delay for 100ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 10);
		}
	}	
	
	
	// Calibrate until the switch is pressed again.
	while (PUSH_BUTTON == 1) {
		
		// Temporary don't use the LED to indicate system error.
		xSystemState.bSkipErrorLed = 1;
		
		
		
		// Calibrate the magnetometer.
		struct MAG_DATA_RAW xMagDataRaw = xReadMagRaw();
		
		if (xMagDataRaw.siMagX > siMaxX) {
			siMaxX = xMagDataRaw.siMagX;
		}	
		if (xMagDataRaw.siMagY > siMaxY) {
			siMaxY = xMagDataRaw.siMagY;
		}	
		if (xMagDataRaw.siMagZ > siMaxZ) {
			siMaxZ = xMagDataRaw.siMagZ;
		}
		
		if (xMagDataRaw.siMagX < siMinX) {
			siMinX = xMagDataRaw.siMagX;
		}	
		if (xMagDataRaw.siMagY < siMinY) {
			siMinY = xMagDataRaw.siMagY;
		}	
		if (xMagDataRaw.siMagZ < siMinZ) {
			siMinZ = xMagDataRaw.siMagZ;
		}	
		
		
		// Blink the LEDs.
		LED1 ^= 1;
		LED2 = 0;
		
		
		
		// If we are calling from task, delay for 10ms to allow other task to run.
		if (xTaskGetCurrentTaskHandle() != NULL) {
			vTaskDelay(configTICK_RATE_HZ / 100);
		}
	}
	
	
	
	// Enable back the LED to indicate system error.
	xSystemState.bSkipErrorLed = 0;
	LED1 = 0;
	LED2 = 0;
	
	// Calculate the Magnetometer offsets.
	xSystemConfig.siMagXMid = (siMinX + siMaxX) / 2;
	xSystemConfig.siMagYMid = (siMinY + siMaxY) / 2;
	xSystemConfig.siMagZMid = (siMinZ + siMaxZ) / 2;
	
	xSystemConfig.fMagXScale = 2.0f / (float)(siMaxX - siMinX);
	xSystemConfig.fMagYScale = 2.0f / (float)(siMaxY - siMinY);
	xSystemConfig.fMagZScale = 2.0f / (float)(siMaxZ - siMinZ);
	
	// Save the calibration data to flash.
	vSaveFlashData();
}	
